/*=+--+=#=+--     Unmanned Aerial System Management Software      --+=#=+--+=#*\
|          Copyright (C) 2011 Regents of the University of Colorado.           |
|                             All Rights Reserved.                             |

     This program is free software: you can redistribute it and/or modify
     it under the terms of the GNU General Public License version 2 as
     published by the Free Software Foundation.

     This program is distributed in the hope that it will be useful,
     but WITHOUT ANY WARRANTY; without even the implied warranty of
     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
     GNU General Public License for more details.

     You should have received a copy of the GNU General Public License
     along with this program.  If not, see <http://www.gnu.org/licenses/>.

            Jack Elston                       Cory Dixon                        
|           elstonj@colorado.edu              dixonc@colorado.edu              |
|                                                                              |
\*=+--+=#=+--                 --+=#=+--+=#=+--                    --+=#=+--+=#*/

/***********************************************************************
 *
 * FILENAME:
 * GPS.cxx
 *
 * PURPOSE:
 *  Provide gps interface
 *
 * CREATED:
 * 02/2005 by Cory Dixon
 *
 * LAST MODIFIED:
 * $Author: dixonc $
 * $Date: 2005/11/02 20:27:20 $
 * $Revision: 1.3 $
 *
 ***********************************************************************/

#include <unistd.h>
#include <stdio.h>
#include <signal.h>
#include <math.h>

#include "GPS.h"
#include "serial.h"
#include "timeLib.h"

#undef SHOW_NMEA

//////////////////////////////////////////////////////////////////////
// Global / Static variables
//////////////////////////////////////////////////////////////////////
#define ZERO_RATE_STR "$PMCAG,005,1,906,000,907,000,GGA,000,GSA,000,RMC,000"
#define RATE_STR      "$PMCAG,005,1,906,001,907,001,GGA,001,GSA,001,RMC,001"

//////////////////////////////////////////////////////////////////////
// GPS 
//////////////////////////////////////////////////////////////////////

//----------------------------------------------------------
// Constructors
//----------------------------------------------------------
// since gps updates only 1 a second, we want
// to run at the rate of the number of strings
// we want, i.e. if two NMEA commands run a 2.0 Hz

void GPSInit(GPS &gps)
{
	gps.utc[0] 		= '\0';	// hhmmss.ss
	gps.epochSec	= 0;	// hhmmss.ss
	gps.date[0]		= '\0';	// ddmmyy
	gps.quality		=  0;	// 0->invalid  1-> fix 2->DGPS
	gps.lat    		= -1;	// deg
	gps.lon    		= -1;	// deg
	gps.alt    		= -1;	// m
	gps.speed  		= -1;	// knots
	gps.track  		= -1;	// ground heading deg from North
	gps.pdop   		= -1;		
	gps.hdop   		= -1;		
	gps.vdop			= -1;		
	for( int i=0; i<GPS_NUM_SATS; i++)
		gps.sats[i].prn = 0;

	//gps.waypointNum	=  100;	// current waypoint
	//gps.bearing		= -1;	// deg from north
	//gps.distance	= -1;	// m to waypoint
	//gps.deltaAlt	= -1;	// m to waypoint
}

//decode=processNMEA( RS232Buf ); // must be null terminated

////////////////////////////////////////////////////////////////////// 
// 
// NMEA Parsing functions
// 
////////////////////////////////////////////////////////////////////// 
int processNMEA(GPS &gps, const char *buf)
{
	VectorString nmea;
	GPSstringTok( buf, nmea, ",*");

	//cout << "processNMEA: " << buf << endl;

	// check the checksum
	string csumStr = nmea[ nmea.size() - 1 ].substr(0,2);
	string calcStr = GPSCalcCheckSumStr( buf, strlen(buf) );

	if( csumStr == calcStr){

		if( nmea[0] == "$GPGGA") {
			GPGGA(gps,nmea);
			return GPGGA_CMD;
		} else if( nmea[0] == "$GPRMC") {
			GPRMC(gps,nmea);
			return GPRMC_CMD;
		} else if( nmea[0] == "$GPGLL") {
			GPGLL(gps,nmea);
			return GPGLL_CMD;
		} else if( nmea[0] == "$GPGSA") {
			GPGSA(gps,nmea);
		} else if( nmea[0] == "$GPGSV") {
			GPGSV(gps,nmea);
		} else if( nmea[0] == "$GPMSS") {
			GPMSS(gps,nmea);
		} else if( nmea[0] == "$GPVTG") {
			GPVTG(gps,nmea);
		} else if( nmea[0] == "$GPZDA") {
			GPZDA(gps,nmea);
		} else if( nmea[0] == "$PMCAG") {
			PMCAG(gps, nmea);
		} else if( nmea[0] == "$PUBX") {
			PUBX(gps,nmea);
			return PUBX_CMD;
		} else {
			cout << "processNMEA - I don't have a parse function for " << nmea[0] << endl;
			return -1;
		}

	} else {
		printf("processNMEA - dropping %i bytes\n", (int)(strlen(buf)));
		cout << "-->> " << buf << " <<--" << endl;
		return -1;
	}

	return 0;
}

void GPSstringTok(const string str, VectorString & tokens, const string& delimiters)
{
	// Skip delimiters at beginning.
	string::size_type start = 0;

	// Find first delimiter
	string::size_type stop  = str.find_first_of(delimiters, start);

	while (string::npos != stop || string::npos != start)
	{
		// Found a token, add it to the vector.
		tokens.push_back(str.substr(start, stop - start));

		// Find next "delimiter"
		if( stop < str.size() ) 
		{
			start = stop + 1;
			stop = str.find_first_of(delimiters, start);
		}
		else
			start = string::npos;
	}
}

uint8_t GPSCalcCheckSum(const char *cmd, int pos)
{
	unsigned char val=0;
	// pg 5-30 of Marconi Manual

	// calc checksum, which is 8 bit xor of all values
	// ignore $ (first character)
	// go to * before checksum	 
	for(int i=1; i < pos && cmd[i] != '*'; i++)
		val ^= cmd[i];

	return val;
}

string GPSCalcCheckSumStr(const char *cmd, int pos)
{
	char temp[3] = { '0' };
	sprintf(temp,"%02X",GPSCalcCheckSum(cmd,pos) );
	temp[2] = '\0';

	string rval = string(temp);
	return rval;
}

uint8_t GPSCalcCheckSum(const string nmea)
{
	// calc checksum, which is 8 bit xor of all values
	// ignore $ (first character)
	// go to * before checksum	 

	uint8_t csum = 0;
	string::size_type start = nmea.find_first_of("$");
	string::size_type stop   = nmea.find_first_of("*");

	if( start == string::npos || stop == string::npos) {
		//cout << "Could not find start/stop of NMEA string" << endl;
		return 0;
	} else
		for( register unsigned int i = start + 1; i < stop; i++)
			csum ^= nmea[i];

	return csum;
}

//char *nmeaCmdStr[GPS_NUM_NMEA_CMDS] = KnownNMEACommandStrs;

void GPZDA(GPS &gps, VectorString & nmea)
{
#ifdef SHOW_NMEA
	cout << "GPZDA\n";
#endif
}

void GPVTG(GPS &gps, VectorString & nmea)
{
#ifdef SHOW_NMEA
	cout << "GPVTG\n";
#endif
}

void GPRMC(GPS &gps, VectorString & nmea)
{
	float tmp;
#ifdef SHOW_NMEA
	cout << "GPRMC\n";
	cout << "GPRMC - utc = "       << nmea[1] << endl;
	cout << "GPRMC - status = "    << nmea[2] << endl;
	cout << "GPRMC - latitude = "  << nmea[3] << endl;
	cout << "GPRMC - N/S = "       << nmea[4] << endl;
	cout << "GPRMC - longitude = " << nmea[5] << endl;
	cout << "GPRMC - E/W = "       << nmea[6] << endl;
	cout << "GPRMC - speed = "     << nmea[7] << endl;
	cout << "GPRMC - track = "     << nmea[8] << endl;
	cout << "GPRMC - date = "      << nmea[9] << endl;
#endif

	if( nmea[2] == "A" ) {
		strcpy(gps.utc,nmea[1].c_str());	// only 9 characters 1 NULL

		tmp = atof(nmea[1].c_str());
		int hh, mm; float ss;
		hh = (int)floor( tmp / 10000.0 ); 
		mm = (int)floor( (tmp - hh*10000) / 100.0 ); 
		ss = (float)( (tmp - hh*10000 - mm*100) ); 
		gps.epochSec = (unsigned long)( hh*3600 + mm*60 + (int)floor(ss));


		tmp= atof(nmea[3].c_str());
		gps.lat = floor(tmp/100) + ((tmp/100 - floor(tmp/100)) * 100/60.0);

		if( nmea[4] == "S" )
			gps.lat *= -1;

		tmp= atof(nmea[5].c_str());
		gps.lon = floor(tmp/100) + ((tmp/100 - floor(tmp/100)) * 100/60.0);

		if( nmea[6] == "W" )
			gps.lon *= -1;

		gps.speed = atof(nmea[7].c_str());
		gps.track = atof(nmea[8].c_str());
		strcpy(gps.date,nmea[9].c_str()); // 6 char and 1 null
	} 
}

void GPMSS(GPS &gps, VectorString & nmea)
{
#ifdef SHOW_NMEA
	cout << "GPMSS\n";
#endif
}

void GPGSV(GPS &gps, VectorString & nmea)
{
	int ind; // keep track of location in NMEAOut

	int msgNum = atoi(nmea[2].c_str());
	int numSat = atoi(nmea[3].c_str());

#ifdef SHOW_NMEA
	cout << "GPGSV\n";
	cout << "GPGSV - number of messages = " << nmea[1] << endl;
	cout << "GPGSV - message number = "     << nmea[2] << endl;
	cout << "GPGSV - satellites in view = " << nmea[3] << endl;


	ind = 4; // keep track of location in NMEAOut
	for(int i=0; i < 4 ; i++){
		if( (msgNum - 1) * 4 + i >= numSat ) break;
		cout << "GPGSV - satellite PRN number = " << nmea[ind++] << endl;
		cout << "GPGSV - elevation (deg) = "      << nmea[ind++] << endl;
		cout << "GPGSV - azimuth (deg) = "        << nmea[ind++] << endl;
		cout << "GPGSV - SNR (db) = "             << nmea[ind++] << endl;
	}
#endif

	ind = 4; // keep track of location in NMEAOut
	for(int i=0; i < 4 ; i++){
		if( (msgNum - 1) * 4 + i >= numSat ) break;
		gps.sats[ (msgNum - 1) * 4 + i].prn       = atoi(nmea[ind++].c_str());
		gps.sats[ (msgNum - 1) * 4 + i].elevation = atof(nmea[ind++].c_str());
		gps.sats[ (msgNum - 1) * 4 + i].azimuth   = atof(nmea[ind++].c_str());
		gps.sats[ (msgNum - 1) * 4 + i].snr       = atof(nmea[ind++].c_str());
	}
}
void GPGSA(GPS &gps, VectorString & nmea)
{

	// data inbetween are satellite prn's
	int numNmea = nmea.size();

#ifdef SHOW_NMEA
	cout << "GPGSA\n";
	cout << "GPGSA - mode = " << nmea[1] << endl;
	cout << "GPGSA - fix = "  << nmea[2] << endl;
	cout << "GPGSA - PDOP = " << nmea[ numNmea - 4] << endl;
	cout << "GPGSA - HDOP = " << nmea[ numNmea - 3] << endl;
	cout << "GPGSA - VDOP = " << nmea[ numNmea - 2] << endl;
#endif

	gps.fix = atoi(nmea[2].c_str());
	gps.pdop = atof(nmea[ numNmea - 4].c_str());
	gps.hdop = atof(nmea[ numNmea - 3].c_str());
	gps.vdop = atof(nmea[ numNmea - 2].c_str());

}
void GPGLL(GPS &gps, VectorString & nmea)
{
	float tmp;

#ifdef SHOW_NMEA
	cout << "GPGLL\n";
	cout << "GPGLL - latitude = "  << nmea[1] << endl;
	cout << "GPGLL - N/S = "       << nmea[2] << endl;
	cout << "GPGLL - longitude = " << nmea[3] << endl;
	cout << "GPGLL - E/W = "       << nmea[4] << endl;
	cout << "GPGLL - utc = "       << nmea[5] << endl;
	cout << "GPGLL - status = "    << nmea[6] << endl;
#endif

	if( nmea[2] == "A" ) {
		tmp= atof(nmea[1].c_str());
		gps.lat = floor(tmp/100) + ((tmp/100 - floor(tmp/100)) * 100/60.0);

		if( nmea[2] == "S" )
			gps.lat *= -1;

		tmp= atof(nmea[3].c_str());
		gps.lon = floor(tmp/100) + ((tmp/100 - floor(tmp/100)) * 100/60.0);

		if( nmea[4] == "W" )
			gps.lon *= -1;

		strcpy(gps.utc,nmea[5].c_str());

		tmp = atof(nmea[5].c_str());
		int hh, mm; float ss;
		hh = (int)floor( tmp / 10000.0 );
		mm = (int)floor( (tmp - hh*10000) / 100.0 );
		ss = (float)( (tmp - hh*10000 - mm*100) );
		gps.epochSec = (unsigned long)( hh*3600 + mm*60 + (int)floor(ss));

	} else
		cout << "GPGLL - data is not valid" << endl;

}

void GPGGA(GPS &gps, VectorString & nmea)
{
	float tmp;

#ifdef SHOW_NMEA
	cout << "GPGGA  - utc time = "             << nmea[1] << endl;
	cout << "GPGGA  - lat = "                  << nmea[2] << endl;
	cout << "GPGGA  - N/S = "                  << nmea[3] << endl;
	cout << "GPGGA  - longitude = "            << nmea[4] << endl;
	cout << "GPGGA  - E/W = "                  << nmea[5] << endl;
	cout << "GPGGA  - qual = "                 << nmea[6] << endl;
	cout << "GPGGA  - sv = "                   << nmea[7] << endl;
	cout << "GPGGA  - hdop = "                 << nmea[8] << endl;
	cout << "GPGGA  - alt = "                  << nmea[9] << endl;
	cout << "GPGGA  - geoidal = "              << nmea[10] << endl;
	cout << "GPGGA  - DGPS age = "             << nmea[11] << endl;
	cout << "GPGGA  - DGPS id = "              << nmea[12] << endl;
#endif

	strcpy(gps.utc,nmea[1].c_str());

	tmp = atof(nmea[1].c_str());
	int hh, mm; float ss;
	hh = (int)floor( tmp / 10000.0 ); 
	mm = (int)floor( (tmp - hh*10000) / 100.0 ); 
	ss = (float)( (tmp - hh*10000 - mm*100) ); 
	gps.epochSec = (unsigned long)( hh*3600 + mm*60 + (int)floor(ss));

	tmp = atof(nmea[2].c_str());
	gps.lat = floor(tmp/100) + ((tmp/100 - floor(tmp/100)) * 100/60.0);
	if( nmea[3] == "S" )
		gps.lat *= -1;
	tmp= atof(nmea[4].c_str());
	gps.lon = floor(tmp/100) + ((tmp/100 - floor(tmp/100)) * 100/60.0);
	if( nmea[5] == "W" )
		gps.lon *= -1;

	gps.quality = atoi(nmea[6].c_str());
	gps.sv      = atoi(nmea[7].c_str());
	gps.hdop    = atof(nmea[8].c_str());
	gps.alt     = atof(nmea[9].c_str());
}

// this is MCM proprioteritory output
void PMCAG(GPS &gps, VectorString & nmea)
{

	// command 902 
	if( nmea[1] == "902" )
	{
#ifdef SHOW_NMEA
		cout << "PMCAG:902\n";
		cout << "PMCAG:902 - software part # = "   << nmea[2] << endl;
		cout << "PMCAG:902 - variation # = "       << nmea[3] << endl;
		cout << "PMCAG:902 - software revision = " << nmea[4] << endl;
		cout << "PMCAG:902 - selftest result = "   << nmea[5] << endl;
		cout << "PMCAG:902 - number of faults = "  << nmea[6] << endl;
		cout << "PMCAG:902 - fault byte = "        << nmea[7] << endl;
#endif
	} else if( nmea[1] == "906" )
	{
#ifdef SHOW_NMEA
		cout << "PMCAG:906\n";
		cout << "PMCAG:906 - zone number = "     << nmea[2] << endl;
		cout << "PMCAG:906 - zone letter = "     << nmea[3] << endl;
		cout << "PMCAG:906 - square column = "   << nmea[4] << endl;
		cout << "PMCAG:906 - square row = "      << nmea[5] << endl;
		cout << "PMCAG:906 - grid easting = "    << nmea[6] << endl;
		cout << "PMCAG:906 - grid northing = "   << nmea[7] << endl;
		cout << "PMCAG:906 - altitude = "        << nmea[8] << endl;
		cout << "PMCAG:906 - waypoint number = " << nmea[9] << endl;
		cout << "PMCAG:906 - true bearing = "    << nmea[10] << endl;
		cout << "PMCAG:907 - square column = " << nmea[4] << endl;
		cout << "PMCAG:907 - square row = "    << nmea[5] << endl;
		cout << "PMCAG:907 - grid easting = "  << nmea[6] << endl;
		cout << "PMCAG:907 - grid northing = " << nmea[7] << endl;
		cout << "PMCAG:907 - altitude = "      << nmea[8] << endl;
		cout << "PMCAG:907 - utc time = "      << nmea[9] << endl;
		cout << "PMCAG:907 - status = "        << nmea[10] << endl;
#endif

		MGRS mgrs;

		// check status if good
		if( nmea[10] == "A" ) {
			gps.alt = atof(nmea[8].c_str());
			strcpy(gps.utc,nmea[9].c_str());

			float tmp;
			tmp = atof(nmea[9].c_str());
			int hh, mm; float ss;
			hh = (int)floor( tmp / 10000.0 );
			mm = (int)floor( (tmp - hh*10000) / 100.0 );
			ss = (float)( (tmp - hh*10000 - mm*100) );
			gps.epochSec = (unsigned long)( hh*3600 + mm*60 + (int)floor(ss));

			mgrs.zoneNum = atoi(nmea[2].c_str());
			mgrs.zoneChar = nmea[3][0];
			mgrs.colChar = nmea[4][0];
			mgrs.rowChar = nmea[5][0];
			mgrs.gridEasting = atoi(nmea[6].c_str());
			mgrs.gridNorthing = atoi(nmea[7].c_str());
			mgrs.altitude = atof(nmea[8].c_str());

		} else
			cout << "PMCAG:907 - data is not valid" << endl;

	} else {
		cout << "PMCAG - unkown command " << nmea[1] << endl;
	}
}

void PUBX(GPS &gps, VectorString & nmea)
{
	float tmp;

#ifdef SHOW_NMEA
	cout << "PUBX  - utc time = "             << nmea[2] << endl;
	cout << "PUBX  - lat = "                  << nmea[3] << endl;
	cout << "PUBX  - N/S = "                  << nmea[4] << endl;
	cout << "PUBX  - longitude = "            << nmea[5] << endl;
	cout << "PUBX  - E/W = "                  << nmea[6] << endl;
	cout << "PUBX  - alt = "                  << nmea[7] << endl;
	cout << "PUBX  - navstat = "              << nmea[8] << endl;
	cout << "PUBX  - hacc = "                 << nmea[9] << endl;
	cout << "PUBX  - vacc = "                 << nmea[10]<< endl;
	cout << "PUBX  - sog = "                  << nmea[11]<< endl;
	cout << "PUBX  - cog = "                  << nmea[12]<< endl;
	cout << "PUBX  - vvel = "                 << nmea[13]<< endl;
	cout << "PUBX  - agec = "                 << nmea[14]<< endl;
	cout << "PUBX  - hdop = "                 << nmea[15]<< endl;
	cout << "PUBX  - vdop = "                 << nmea[16]<< endl;
	cout << "PUBX  - tdop = "                 << nmea[17]<< endl;
	cout << "PUBX  - gu = "                   << nmea[18]<< endl;
	cout << "PUBX  - ru = "                   << nmea[19]<< endl;
	cout << "PUBX  - rd = "                   << nmea[20]<< endl;
	cout << endl;
#endif

	strcpy(gps.utc,nmea[2].c_str());

	tmp = atof(nmea[2].c_str());
	int hh, mm; float ss;
	hh = (int)floor( tmp / 10000.0 ); 
	mm = (int)floor( (tmp - hh*10000) / 100.0 ); 
	ss = (float)( (tmp - hh*10000 - mm*100) ); 
	gps.epochSec = (unsigned long)( hh*3600 + mm*60 + (int)floor(ss));

	tmp = atof(nmea[3].c_str());
	gps.lat = floor(tmp/100) + ((tmp/100 - floor(tmp/100)) * 100/60.0);
	if( nmea[4] == "S" )
		gps.lat *= -1;
	tmp= atof(nmea[5].c_str());
	gps.lon = floor(tmp/100) + ((tmp/100 - floor(tmp/100)) * 100/60.0);
	if( nmea[6] == "W" )
		gps.lon *= -1;

	gps.alt = atof(nmea[7].c_str());
}
